import threading, Queue, random, thread,time
from math import *
from numpy import *
from numpy.linalg import *
from path_finder import *
seterr(all='raise') # Set the numpy default error level to raise
# MATH CONVENTION
# For now, the convention of the coordinate system is the first dimension of a position/displacement is X, the second is Y (i.e. [x,y]).  The top left corner of the window is
# the world coordinate system origin, Y grows as you go down the window and X grows as you go right on the window.  Positive rotation occurs in a CCW direction.
# For coordinate systems relative to the agent, forward is +Y axis, back is -Y axis, right is +X axis, and left is -X axis.  Turning left is a positive angle, and turning
# right is a negative angle.  Thus, a change a positive change in angle in the world is both a CCW rotation and to the agent turning left.
# World heading of 0 is to the right, world heading of pi/2 is down (parallel to the +Y axis)

# Agent Coordinate System
# Agent's heading is the positive Y axis.  Given a displacement vector disp=[x_0,y_0], then the angle atan2(y_0,x_0) is from the positive X axis of the agent's coordinate system.

# The world model keeps track of position/velocity/acceleration of the agents, handles collisions with other agents, walls, etc.  The model is connected to by the agent to control it's desired position and headings in several different formats.  This is also used by the agent_controller to do the same tasks the agent would like to do and also enter a debug mode that allows for special actions.
class world_model(object):
	def __init__(self,world_graphics_engine,walls=[],max_velocity_mag=20,max_heading_diff=pi/4):
		self.walls=walls # List of tuples, x_0,y_0,x_1,y_1
		self.wall_width=2
		self.max_velocity_mag=max_velocity_mag
		self.max_heading_diff=max_heading_diff
		self.graphics_engine=world_graphics_engine
		self.make_graph(3)
		
		# Agent Specific Settings, all indexed by agent_num
		self.agent_position={}
		self.agent_heading={}
		self.agent_sensor_attributes={} # Used to tell the graphics the sensor information for display purposes
		
		# Create graphical representation of world_model
		self.graphics_engine.create_walls(self.walls,self.wall_width)

	def make_graph(self,width_mult=2):
		"""Populates a graph with nodes and connections based on the walls defined in the world model. Creates nodes around the endpoints of each wall, and creates connections between any two nodes that have no obstructions between them. Returns a dict that uses each node's location as a key, and a list of adjacent nodes as each node's value."""
		self.graph={}
		width=width_mult*self.wall_width
		for wall in self.walls:
			self.graph[(wall[0]-width,wall[1]-width)]=[]
			self.graph[(wall[0]+width,wall[1]-width)]=[]
			self.graph[(wall[0]-width,wall[1]+width)]=[]
			self.graph[(wall[0]+width,wall[1]+width)]=[]
			self.graph[(wall[2]-width,wall[3]-width)]=[]
			self.graph[(wall[2]+width,wall[3]-width)]=[]
			self.graph[(wall[2]-width,wall[3]+width)]=[]
			self.graph[(wall[2]+width,wall[3]+width)]=[]
		# Populate each node's adjacency list by checking whether or not the line to another node is obstructed.
		for node in self.graph:
			for node_2 in self.graph:
				if self.exists_clear_path_between(node, node_2):
					self.graph[node].append(node_2)
	def rel_agent_locations(self,agent_num):
		"""This returns an angle and magnitude describing the relative heading and distance to each agent from the provided agent.  The angle is relative to the agent's heading (heading = +Y axis = 0 degrees, objects to the right of the agent are 0-pi and -pi-0 on the left)."""
		result=[]
		pos=self.agent_position[agent_num]
		agent_heading=self.agent_heading[agent_num]
		for i in self.agent_position.iterkeys():
			if i == agent_num:
				continue
			cur_pos=self.agent_position[i]
			temp_diff=[pos[0]-cur_pos[0],pos[1]-cur_pos[1]]
			dist=sqrt(temp_diff[0]*temp_diff[0]+temp_diff[1]*temp_diff[1])
			heading=((atan2(temp_diff[1],temp_diff[0])-agent_heading) % (2*pi))-pi
			result.append([heading,dist])
		return result
	def rel_node_locations(self,agent_num):
		"""This returns an angle and magnitude describing the relative heading and distance to each graph node from the provided agent.  The angle is relative to the agent's heading (heading = +Y axis = 0 degrees, objects to the right of the agent are 0-pi and -pi-0 on the left)."""
		result=[]
		pos=self.agent_position[agent_num]
		agent_heading=self.agent_heading[agent_num]
		for node in self.graph.iterkeys():
			temp_diff=[pos[0]-node[0],pos[1]-node[1]]
			dist=sqrt(temp_diff[0]*temp_diff[0]+temp_diff[1]*temp_diff[1])
			heading=((atan2(temp_diff[1],temp_diff[0])-agent_heading) % (2*pi))-pi
			result.append([heading,dist])
		return result
	def exists_clear_path_between(self, pos_0, pos_1):
		"""Returns True if there is no wall between two points; False otherwise."""
		y_dif = pos_1[1]-pos_0[1]
		x_dif = pos_1[0]-pos_0[0]
		if self.min_collision_dist(pos_0, atan2(y_dif,x_dif)) >= float(norm(array([x_dif,y_dif]))):
			return True
		return False
	def min_collision_dist_rel(self,agent_num,relative_heading=0):
		return self.min_collision_dist(self.agent_position[agent_num], self.agent_heading[agent_num]+relative_heading)
	def min_collision_dist(self, position, angle):
		pos_0=array([position[0],position[1],1.0])
		pos_1=array([cos(angle)/self.wall_width,sin(angle)/self.wall_width,0.0])+pos_0
		obj_line=cross(pos_0,pos_1)
		obj_segment=tuple(position)+(pos_1[0],pos_1[1])
		min_distance=float(1e10000)
		for wall in self.walls:
			wall=(float(wall[0]),float(wall[1]),float(wall[2]),float(wall[3]))
			wall_pos0=array([wall[0],wall[1],1.0])
			wall_pos1=array([wall[2],wall[3],1.0])
			wall_line=cross(wall_pos0,wall_pos1)
			collision_point=cross(obj_line,wall_line)
			try:
				collision_point=collision_point/collision_point[2]
			except FloatingPointError,err:
				continue
			if self.point_on_segment(collision_point,wall) and self.is_monotonic(pos_0[0:2],pos_1[0:2],collision_point[0:2]):
				min_distance=min(norm(pos_0[0:2]-collision_point[0:2])-self.wall_width*1.5,min_distance)
		return float(min_distance)
	@staticmethod
	def is_monotonic(pnt0,pnt1,pnt2):
		"""Given 3 points (numpy arrays), this will return true if dist(pnt0,pnt2)>dist(pnt1,pnt2), this assumes colinearity."""
		temp_diff0=pnt2-pnt0
		temp_diff1=pnt2-pnt1
		return sum(temp_diff0*temp_diff0) > sum(temp_diff1*temp_diff1)
	@staticmethod
	def point_on_segment(point,line_seg): # NOTE This point must lie on the line passing through the line_seg or else...
		"""Given a point that intersects with the line that passes through the line segment, this will return true if it lies on the line segment"""
		eps=.00001
		if (line_seg[0]-eps<=point[0]<=line_seg[2]+eps or line_seg[0]+eps>=point[0]>=line_seg[2]-eps) and (line_seg[1]-eps<=point[1]<=line_seg[3]+eps or line_seg[1]+eps>=point[1]>=line_seg[3]-eps):
			return True
		return False
	def new_agent(self,agent_num,pos=None,heading=None,sensor_attributes=None):
		if pos==None:
			pos=self.graphics_engine.random_pos()
		if heading==None:
			print('Heading:')
			print(heading)
			heading=random.random()*2*pi
		"""Creates a new agent at the end position"""
		self.agent_position[agent_num]=pos
		self.agent_heading[agent_num]=heading
		self.agent_sensor_attributes[agent_num]=sensor_attributes
		self.update_pos(agent_num)
	def remove_agent(self,agent_num):
		"""Remove all traces of a specified agent number"""
		self.graphics_engine.remove_agent(agent_num)
		del self.agent_position[agent_num]
		del self.agent_heading[agent_num]
		del self.agent_sensor_attributes[agent_num]
	# These are to be interfaced with by the agents
	def agent_move_rel_world(self,agent_num,disp):
		try:
			temp_disp_mag=sqrt(disp[0]*disp[0]+disp[1]*disp[1]) # Compute vector magnitude
		except OverflowError:
			temp_disp_mag=float('inf')
		
		temp_angle=atan2(disp[1],disp[0])
		temp_disp_mag=min([self.max_velocity_mag,temp_disp_mag*self.max_velocity_mag,self.min_collision_dist_rel(agent_num,temp_angle-self.agent_heading[agent_num])])
		disp[0]=cos(temp_angle)*temp_disp_mag
		disp[1]=sin(temp_angle)*temp_disp_mag

		self.agent_position[agent_num][0]+=disp[0]
		self.agent_position[agent_num][1]+=disp[1]
		self.update_pos(agent_num)
	def agent_move_rel_agent(self,agent_num,disp):
		"""This takes the agent heading and displacement vector, both off of the +X axis, and computes the angle off of the agent heading (heading-disp).
		Positive angles have a motion towards the left of the agent."""
		try:
			temp_disp_mag=sqrt(disp[0]*disp[0]+disp[1]*disp[1]) # Compute vector magnitude
		except OverflowError:
			temp_disp_mag=float('inf')

		temp_angle=atan2(disp[1],disp[0])
		temp_disp_mag=min([self.max_velocity_mag,temp_disp_mag,self.min_collision_dist_rel(agent_num,(pi/2-temp_angle))])
		disp[0]=sin(temp_angle-self.agent_heading[agent_num])*temp_disp_mag # same as cos(self.agent_heading[agent_num]+(pi/2-temp_angle))*temp_disp_mag
		disp[1]=cos(temp_angle-self.agent_heading[agent_num])*temp_disp_mag # same as sin(self.agent_heading[agent_num]+(pi/2-temp_angle))*temp_disp_mag
		self.agent_position[agent_num][0]+=disp[0]
		self.agent_position[agent_num][1]+=disp[1]
		self.update_pos(agent_num)
	def agent_turn_rel(self,agent_num,heading_diff):
		self.agent_heading[agent_num]+=max(min(heading_diff,self.max_heading_diff),-self.max_heading_diff)
		if self.agent_heading[agent_num] >= 0:
			self.agent_heading[agent_num]=self.agent_heading[agent_num] % (2*pi)
		else:
			self.agent_heading[agent_num]=self.agent_heading[agent_num] % (-2*pi)
		self.update_pos(agent_num)
	def seek(self, agent_num, dest):
		# Disp to target
		y_del = dest[1]-self.agent_position[agent_num][1]
		x_del = dest[0]-self.agent_position[agent_num][0]
		
		# Angle to target
		heading = atan2(y_del, x_del)
		dist = norm([y_del,x_del])
		# If we aren't pointing in the right direction than fix it
		if self.agent_heading[agent_num] != heading:
			# Corrects for problem when heading-agent_heading is greater than the other way around
			self.agent_turn_rel(agent_num, (min(heading-self.agent_heading[agent_num],2*pi-heading-self.agent_heading[agent_num])))
		# Else go straight ahead
		else:
			self.agent_move_rel_agent(agent_num, [0,dist])

	# These are to be used during debugging and by the game_model
	def debug_set_pos(self,agent_num,pos):
		self.agent_position[agent_num]=pos
		self.update_pos(agent_num)
	def debug_set_heading(self,agent_num,heading):
		self.agent_heading[agent_num]=heading
		self.update_pos(agent_num)
	def update_pos(self,agent_num):
		self.debug_print_agent_data(agent_num)
		self.graphics_engine.update_pos(agent_num,self.agent_position[agent_num],self.agent_heading[agent_num])
	def debug_print_agent_data(self,agent_num):
		print('Agent[%d] Position(%f,%f) Heading(%f)') %(agent_num,self.agent_position[agent_num][0],self.agent_position[agent_num][1],self.agent_heading[agent_num])
	# TODO Decide on how to implement the absolute positionings
	def agent_move_abs(self,agent_num):
		None
	def agent_rotate_abs(self,agent_num):
		None
